
/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>

#include <string>
#include <set>

#include <QtCore>
#include <QtGui>
#include <QPluginLoader>
#include <QApplication>

#include <base/utils/utils.h>
#include <hardware/Gps/GPS.h>
#include <hardware/Gps/utils_GPS.h>
#include <cv/highgui/VideoReader.h>
#include <cv/highgui/VideoPOS_Transfer.h>
#include <network/MessagePassing.h>
#include <hardware/Joystick/HAL_JSCombo.h>
#include <uav/UAS.h>
#include <uav/utils_mavlink.h>
#include <uav/VirtualUAV.h>
#include <uav/VirtualUAV_Quad.h>
#include <uav/FlightData_Transfer.h>
#include <uav/MissionData_Transfer.h>


#include "test.h"


using namespace std;
using namespace pi;

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

#define PACKET_STASTISTIC_NUM   100

class packet_stastistic
{
public:
    ru64    tm_last, tm_now, tm_delta;
    float   tm_min, tm_max, tm_mean;

    ru64    tm_all[PACKET_STASTISTIC_NUM];
    ri32    tm_n, tm_idx;

public:
    packet_stastistic() {
        init();
    }

    ~packet_stastistic() {
        release();
    }

    void reset(void) {
        release();
    }

    void packet_recved(void) {
        tm_now  = tm_get_ms();

        if( tm_n < 0 ) {
            tm_last = tm_now;
            tm_n ++;
            return;
        } else {
            tm_n ++;
        }

        // calculate time delta
        tm_delta = tm_now - tm_last;
        tm_last  = tm_now;
        tm_all[tm_idx%PACKET_STASTISTIC_NUM] = tm_delta;
        tm_idx ++;

        // determin min/max delta
        if( tm_delta < tm_min ) tm_min = tm_delta;
        if( tm_delta > tm_max ) tm_max = tm_delta;

        // calc mean delta
        int     i, n;
        float   tm_sum = 0;
        if( tm_n <= PACKET_STASTISTIC_NUM )
            n = tm_n;
        else
            n = PACKET_STASTISTIC_NUM;

        for(i=0; i<n; i++) {
            tm_sum = tm_sum + tm_all[i];
        }
        tm_mean = tm_sum / n;
    }

    void init(void) {
        tm_last  = 0;
        tm_now   = 0;
        tm_delta = 0;
        tm_min   = 9999;
        tm_max   = 0;
        tm_mean  = 0;

        tm_n     = -1;
        tm_idx   = 0;
    }

    void release() {
        tm_n   = -1;
        tm_idx = 0;

        tm_min   = 9999;
        tm_max   = 0;
        tm_mean  = 0;
    }
};


int test_mavlink_reading(CParamArray *pa)
{
    string          port = "/dev/ttyACM0";
    int             baud = 115200;
    UART            uart;

    uint8_t         ub;
    int             i, j;
    int             ret;

    int                 packet_drops = 0;
    packet_stastistic   packetSta[256];

    mavlink_message_t   msg;
    mavlink_status_t    status;

    std::set<int>       msgid_set;

    // parase input arguments
    port = svar.GetString("port", port);
    baud = svar.GetInt("baud", baud);

    // open UART port
    uart.port_name = port;
    uart.baud_rate = baud;
    ret = uart.open();
    if( ret != 0 ) {
        dbg_pe("Can not open UART port: %s\n", port.c_str());
        return -1;
    }

    // begin receiving
    i = 0;
    while(1) {
        // read one char
        ret = uart.read(&ub, 1);
        if( ret < 0 ) continue;

        // parse received data
        if( mavlink_parse_char(MAVLINK_COMM_0, ub, &msg, &status) ) {

            i++;

            // stastistic packing receiving timing
            packetSta[msg.msgid].packet_recved();

            // insert msg id set
            msgid_set.insert(msg.msgid);

            //if( i > 500 ) break;

            printf("sysid = %3d, compid = %3d, msgid = %3d,"
                   "len = %3d, seq = %3d, tm(dt, min, max, mean)"
                   "= %6d, %6.1f, %6.1f, %6.1f\n",
                   msg.sysid, msg.compid,
                   msg.msgid, msg.len, msg.seq,
                   packetSta[msg.msgid].tm_delta,
                   packetSta[msg.msgid].tm_min, packetSta[msg.msgid].tm_max,
                   packetSta[msg.msgid].tm_mean);

            if( svar.GetInt("mavlink.showDetailedMsg", 0) )
            {
                // MAVLINK_MSG_ID_HEARTBEAT = 0
                if( msg.msgid == MAVLINK_MSG_ID_HEARTBEAT ) {
                    mavlink_heartbeat_t hb;
                    mavlink_msg_heartbeat_decode(&msg, &hb);

                    printf("    custom_mode   = %d\n", hb.custom_mode);
                    printf("    type          = %d\n", hb.type);
                    printf("    autopilot     = %d\n", hb.autopilot);
                    printf("    base_mode     = %d\n", hb.base_mode);
                    printf("    system_status = %d\n", hb.system_status);
                    printf("    mavlink_ver   = %d\n\n", hb.mavlink_version);
                }

                // MAVLINK_MSG_ID_SYS_STATUS = 1
                if( msg.msgid == MAVLINK_MSG_ID_SYS_STATUS ) {
                    mavlink_sys_status_t ss;
                    mavlink_msg_sys_status_decode(&msg, &ss);

                    printf("    onboard_control_sensors_present = %d\n", ss.onboard_control_sensors_present);
                    printf("    onboard_control_sensors_enabled = %d\n", ss.onboard_control_sensors_enabled);
                    printf("    onboard_control_sensors_health  = %d\n", ss.onboard_control_sensors_health);
                    printf("    load                            = %d\n", ss.load);
                    printf("    voltage_battery                 = %d\n", ss.voltage_battery);
                    printf("    current_battery                 = %d\n", ss.current_battery);
                    printf("    drop_rate_comm                  = %d\n", ss.drop_rate_comm);
                    printf("    errors_comm                     = %d\n", ss.errors_comm);
                    printf("    errors_count1                   = %d\n", ss.errors_count1);
                    printf("    errors_count2                   = %d\n", ss.errors_count2);
                    printf("    errors_count3                   = %d\n", ss.errors_count3);
                    printf("    errors_count4                   = %d\n", ss.errors_count4);
                    printf("    battery_remaining               = %d\n", ss.battery_remaining);

                    mavlink_sys_status_sensor_list  idPresent, idEnabled, idHealth, idUnhealth;
                    mavlink_sys_status_sensor_namelist  nameList;

                    mavlink_sys_status_sensor_getIDs(ss.onboard_control_sensors_present, idPresent);
                    mavlink_sys_status_sensor_getNames(idPresent, nameList);
                    printf("    onboard_control_sensors_present = ");
                    for(j=0; j<nameList.size(); j++) {
                        printf("%s, ", nameList[j].c_str());
                    }
                    printf("\n");

                    mavlink_sys_status_sensor_getIDs(ss.onboard_control_sensors_enabled, idEnabled);
                    mavlink_sys_status_sensor_getNames(idEnabled, nameList);
                    printf("    onboard_control_sensors_enabled = ");
                    for(j=0; j<nameList.size(); j++) {
                        printf("%s, ", nameList[j].c_str());
                    }
                    printf("\n");

                    mavlink_sys_status_sensor_getIDs(ss.onboard_control_sensors_health, idHealth);
                    mavlink_sys_status_sensor_getNames(idHealth, nameList);
                    printf("    onboard_control_sensors_health = ");
                    for(j=0; j<nameList.size(); j++) {
                        printf("%s, ", nameList[j].c_str());
                    }
                    printf("\n");

                    mavlink_sys_status_sensor_getID_Difference(idPresent, idHealth, idUnhealth);
                    mavlink_sys_status_sensor_getNames(idUnhealth, nameList);
                    printf("    onboard_control_sensors_unhealth = ");
                    for(j=0; j<nameList.size(); j++) {
                        printf("%s, ", nameList[j].c_str());
                    }
                    printf("\n");

                    printf("\n");
                }

                // MAVLINK_MSG_ID_SYSTEM_TIME = 2
                if( msg.msgid == MAVLINK_MSG_ID_SYSTEM_TIME ) {
                    mavlink_system_time_t st;
                    mavlink_msg_system_time_decode(&msg, &st);

                    printf("    time_unix_usec  = %lld\n", st.time_unix_usec);
                    printf("    time_boot_ms    = %lld\n\n", st.time_boot_ms);
                }

                // MAVLINK_MSG_ID_GPS_RAW_INT = 24
                if( msg.msgid == MAVLINK_MSG_ID_GPS_RAW_INT ) {
                    mavlink_gps_raw_int_t gps;
                    mavlink_msg_gps_raw_int_decode(&msg, &gps);

                    printf("    time_usec               = %lld\n", gps.time_usec);
                    printf("    lat                     = %d\n", gps.lat);
                    printf("    lon                     = %d\n", gps.lon);
                    printf("    alt                     = %d\n", gps.alt);
                    printf("    eph                     = %d\n", gps.eph);
                    printf("    epv                     = %d\n", gps.epv);
                    printf("    vel                     = %d\n", gps.vel);
                    printf("    cog                     = %d\n", gps.cog);
                    printf("    fix_type                = %d\n", gps.fix_type);
                    printf("    satellites_visible      = %d\n\n", gps.satellites_visible);
                }

                // MAVLINK_MSG_ID_RAW_IMU = 27
                if( msg.msgid == MAVLINK_MSG_ID_RAW_IMU ) {
                    mavlink_raw_imu_t pack;
                    mavlink_msg_raw_imu_decode(&msg, &pack);

                    printf("    xacc                    = %d\n", pack.xacc);
                    printf("    yacc                    = %d\n", pack.yacc);
                    printf("    zacc                    = %d\n", pack.zacc);
                    printf("    xgyro                   = %d\n", pack.xgyro);
                    printf("    ygyro                   = %d\n", pack.ygyro);
                    printf("    zgyro                   = %d\n", pack.zgyro);
                    printf("    xmag                    = %d\n", pack.xmag);
                    printf("    ymag                    = %d\n", pack.ymag);
                    printf("    zmag                    = %d\n\n", pack.zmag);
                }

                // MAVLINK_MSG_ID_SCALED_IMU2 = 116
                if( msg.msgid == MAVLINK_MSG_ID_SCALED_IMU2 ) {
                    mavlink_scaled_imu2_t pack;
                    mavlink_msg_scaled_imu2_decode(&msg, &pack);

                    printf("    time_boot_ms            = %d\n", pack.time_boot_ms);
                    printf("    xacc                    = %d\n", pack.xacc);
                    printf("    yacc                    = %d\n", pack.yacc);
                    printf("    zacc                    = %d\n", pack.zacc);
                    printf("    xgyro                   = %d\n", pack.xgyro);
                    printf("    ygyro                   = %d\n", pack.ygyro);
                    printf("    zgyro                   = %d\n", pack.zgyro);
                    printf("    xmag                    = %d\n", pack.xmag);
                    printf("    ymag                    = %d\n", pack.ymag);
                    printf("    zmag                    = %d\n\n", pack.zmag);
                }

                // MAVLINK_MSG_ID_ATTITUDE = 30
                if( msg.msgid == MAVLINK_MSG_ID_ATTITUDE ) {
                    mavlink_attitude_t att;
                    mavlink_msg_attitude_decode(&msg, &att);

                    printf("    time_boot_ms = %d\n", att.time_boot_ms);
                    printf("    roll = %12f, pitch = %12f, yaw = %12f\n\n",
                           att.roll, att.pitch, att.yaw);
                }

                // MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 33
                if( msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT ) {
                    mavlink_global_position_int_t gp;
                    mavlink_msg_global_position_int_decode(&msg, &gp);

                    printf("    time_boot_ms        = %d\n", gp.time_boot_ms);
                    printf("    lat                 = %d\n", gp.lat);
                    printf("    lon                 = %d\n", gp.lon);
                    printf("    alt                 = %d\n", gp.alt);
                    printf("    relative_alt        = %d\n", gp.relative_alt);
                    printf("    vx                  = %d\n", gp.vx);
                    printf("    vy                  = %d\n", gp.vy);
                    printf("    vz                  = %d\n", gp.vz);
                    printf("    hdg                 = %d\n\n", gp.hdg);
                }

                // MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35
                if( msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW ) {
                    mavlink_rc_channels_raw_t rc;
                    mavlink_msg_rc_channels_raw_decode(&msg, &rc);

                    printf("    time_boot_ms        = %d\n", rc.time_boot_ms);
                    printf("    chan1_raw           = %d\n", rc.chan1_raw);
                    printf("    chan2_raw           = %d\n", rc.chan2_raw);
                    printf("    chan3_raw           = %d\n", rc.chan3_raw);
                    printf("    chan4_raw           = %d\n", rc.chan4_raw);
                    printf("    chan5_raw           = %d\n", rc.chan5_raw);
                    printf("    chan6_raw           = %d\n", rc.chan6_raw);
                    printf("    chan7_raw           = %d\n", rc.chan7_raw);
                    printf("    chan8_raw           = %d\n", rc.chan8_raw);
                    printf("    port                = %d\n", rc.port);
                    printf("    rssi                = %d\n\n", rc.rssi);
                }

                // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 36
                if( msg.msgid == MAVLINK_MSG_ID_SERVO_OUTPUT_RAW ) {
                    mavlink_servo_output_raw_t so;
                    mavlink_msg_servo_output_raw_decode(&msg, &so);

                    printf("    time_usec           = %d\n", so.time_usec);
                    printf("    servo1_raw          = %d\n", so.servo1_raw);
                    printf("    servo2_raw          = %d\n", so.servo2_raw);
                    printf("    servo3_raw          = %d\n", so.servo3_raw);
                    printf("    servo4_raw          = %d\n", so.servo4_raw);
                    printf("    servo5_raw          = %d\n", so.servo5_raw);
                    printf("    servo6_raw          = %d\n", so.servo6_raw);
                    printf("    servo7_raw          = %d\n", so.servo7_raw);
                    printf("    servo8_raw          = %d\n", so.servo8_raw);
                    printf("    port                = %d\n\n", so.port);
                }

                // MAVLINK_MSG_ID_MISSION_CURRENT = 42
                if( msg.msgid == MAVLINK_MSG_ID_MISSION_CURRENT ) {
                    mavlink_mission_current_t   pack;
                    mavlink_msg_mission_current_decode(&msg, &pack);

                    printf("    seq                 = %d\n\n", pack.seq);
                }

                // MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62
                if( msg.msgid == MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT) {
                    mavlink_nav_controller_output_t nco;
                    mavlink_msg_nav_controller_output_decode(&msg, &nco);

                    printf("    nav_roll            = %f\n", nco.nav_roll);
                    printf("    nav_pitch           = %f\n", nco.nav_pitch);
                    printf("    alt_error           = %f\n", nco.alt_error);
                    printf("    aspd_error          = %f\n", nco.aspd_error);
                    printf("    xtrack_error        = %f\n", nco.xtrack_error);
                    printf("    nav_bearing         = %d\n", nco.nav_bearing);
                    printf("    target_bearing      = %d\n", nco.target_bearing);
                    printf("    wp_dist             = %d\n\n", nco.wp_dist);
                }

                // MAVLINK_MSG_ID_VFR_HUD = 74
                if( msg.msgid == MAVLINK_MSG_ID_VFR_HUD ) {
                    mavlink_vfr_hud_t   vfr;
                    mavlink_msg_vfr_hud_decode(&msg, &vfr);

                    printf("    airspeed            = %f\n", vfr.airspeed);
                    printf("    groundspeed         = %f\n", vfr.groundspeed);
                    printf("    alt                 = %f\n", vfr.alt);
                    printf("    climb               = %f\n", vfr.climb);
                    printf("    heading             = %d\n", vfr.heading);
                    printf("    throttle            = %d\n\n", vfr.throttle);
                }

                // MAVLINK_MSG_ID_POWER_STATUS = 125
                if( msg.msgid == MAVLINK_MSG_ID_POWER_STATUS ) {
                    mavlink_power_status_t pack;
                    mavlink_msg_power_status_decode(&msg, &pack);

                    printf("    Vcc                 = %d\n", pack.Vcc);
                    printf("    Vservo              = %d\n", pack.Vservo);
                    printf("    flags               = %x\n\n", pack.flags);
                }
            }
        }

        packet_drops += status.packet_rx_drop_count;
    }

    // print all msgid
    printf("MSG ID = ");
    set<int>::iterator it;
    for(it=msgid_set.begin(); it!=msgid_set.end(); it++) {
        printf("%d, ", *it);
    }
    printf("\n");

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_GCP_DataAnalysis(CParamArray *pa)
{
    FILE                *fp = NULL;
    char                buf[256];

    uint8_t             ub;
    int                 i, j;

    int                 printMsg = 1;

    int                 sysID, compID;

    mavlink_message_t   msg;
    mavlink_status_t    status;

    double              lat, lng, alt;
    double              latSum, lngSum, altSum;
    int                 nData = 0;


    // parase input arguments
    string fn_in  = svar.GetString("fn_in", "");
    string fn_out = svar.GetString("fn_out", "");

    sysID  = svar.GetInt("sysID", -1);
    compID = svar.GetInt("compID", 0);

    printMsg = svar.GetInt("MAVLINK.printMsg", 1);

    // process file name
    if( fn_in.size() > 0 ) {
        if( fn_out.size() <= 0 ) fn_out = fmt::sprintf("%s_GCP.txt", fn_in);
    } else {
        return -2;
    }

    // open file
    fp = fopen(fn_in.c_str(), "rt");
    if( !fp ) {
        dbg_pe("Can not open input file: %s\n", fn_in.c_str());
        return -1;
    }

    latSum = 0;
    lngSum = 0;
    altSum = 0;

    // begin receiving
    i = 0;
    while(1) {
        j = fread(buf, 1, 1, fp);
        if( j <= 0 ) break;

        ub = buf[0];

        // parse received data
        if( mavlink_parse_char(MAVLINK_COMM_0, ub, &msg, &status) ) {

            i++;

            if( sysID >= 0 ) {
                if( sysID != msg.sysid ) continue;
            }

            // stastistic packing receiving timing
            if( printMsg )
                printf("sysid = %3d, compid = %3d, msgid = %3d,"
                       "len = %3d, seq = %3d\n",
                       msg.sysid, msg.compid,
                       msg.msgid, msg.len, msg.seq);

            // MAVLINK_MSG_ID_GPS_RAW_INT = 24
            if( msg.msgid == MAVLINK_MSG_ID_GPS_RAW_INT ) {
                mavlink_gps_raw_int_t gps;
                mavlink_msg_gps_raw_int_decode(&msg, &gps);

                if( gps.fix_type < 3 || gps.satellites_visible < 8 ) continue;

                printf("    time_usec               = %lld\n", gps.time_usec);
                printf("    lat                     = %d\n", gps.lat);
                printf("    lon                     = %d\n", gps.lon);
                printf("    alt                     = %d\n", gps.alt);
                printf("    eph                     = %d\n", gps.eph);
                printf("    epv                     = %d\n", gps.epv);
                printf("    vel                     = %d\n", gps.vel);
                printf("    cog                     = %d\n", gps.cog);
                printf("    fix_type                = %d\n", gps.fix_type);
                printf("    satellites_visible      = %d\n\n", gps.satellites_visible);

                latSum += 1.0*gps.lat/1e7;
                lngSum += 1.0*gps.lon/1e7;
                altSum += 1.0*gps.alt/1000.0;

                nData ++;
            }
        }
    }

    fclose(fp);


    lat = latSum / nData;
    lng = lngSum / nData;
    alt = altSum / nData;

    // print results
    printf("\n");
    printf("N   = %d\n", nData);
    printf("lat = %14.8f\n", lat);
    printf("lon = %14.8f\n", lng);
    printf("alt = %14.8f\n", alt);

    if( fn_out.length() > 0 ) {
        fp = fopen(fn_out.c_str(), "wt");
        if( !fp ) return -1;

        fprintf(fp, "lat = %14.8f\n", lat);
        fprintf(fp, "lon = %14.8f\n", lng);
        fprintf(fp, "alt = %14.8f\n", alt);

        fclose(fp);
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_uart(CParamArray *pa)
{
    string          port = "/dev/ttyUSB0";
    int             baud = 115200;
    UART            *uart;

    uint8_t         ub;
    int             i, j, s, l, rl;
    int             ret;

    // parase input arguments
    port = svar.GetString("port", port);
    baud = svar.GetInt("baud", baud);

    // open UART port
    uart = new UART;
    uart->port_name = port;
    uart->baud_rate = baud;

    ret = uart->open();
    if( ret != 0 ) {
        dbg_pe("Can not open UART port: %s\n", port.c_str());
        return -1;
    }

    // begin receiving
    i = 0;
    j = 0;
    s = 0;

    printf("[%05d] ", j++);

    while(1) {
        // read one char
        ret = uart->read(&ub, 1);
        if( ret <= 0 ) continue;

        printf("%02X %d ", ub, s);

        if( s == 0 ) {
            if( ub == 0xA5 ) s = 1;
        } else if ( s == 1 ) {
            if( ub == 0x5A ) {
                s = 2;
                l = 0;
                rl = 0;
            } else
                s = 0;
        } else if ( s == 2 ) {
            if( rl == 0 ) {
                l = ub;
            }

            rl ++;

            if( rl == l ) {
                printf("\n");
                printf("[%05d] ", j++);
                s = 0;
            }
        }

        //i++;
        //if( i % 40 == 0 ) printf("\n");
    }

    delete uart;

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_gps(CParamArray *pa)
{
    string          port = "/dev/ttyACM0";
    int             baud = 115200;
    int             port_type = 0;
    UART            *uart;

    uint8_t         ub;
    int             i;
    int             ret;

    mavlink_message_t   msg;
    mavlink_status_t    status;

    double              lat0, lng0, lat1, lng1,
        dx0, dy0, dx1, dy1, dx2, dy2;

    // parase input arguments
    port = svar.GetString("port", port);
    baud = svar.GetInt("baud", baud);
    port_type = svar.GetInt("port_type", port_type);

    // open UART port
    if( port_type == 0 )
        uart = new UART;
    else
        uart = new VirtualUART;

    uart->port_name = port;
    uart->baud_rate = baud;
    ret = uart->open();
    if( ret != 0 ) {
        dbg_pe("Can not open UART port: %s\n", port.c_str());
        return -1;
    }

    // begin receiving
    i = 0;
    while(1) {
        // read one char
        ret = uart->read(&ub, 1);
        if( ret < 0 ) continue;

        // parse received data
        if( mavlink_parse_char(MAVLINK_COMM_0, ub, &msg, &status) ) {

#if 0
            printf("sysid = %3d, compid = %3d, msgid = %3d,"
                   "len = %3d, seq = %3d\n",
                    msg.sysid, msg.compid,
                    msg.msgid, msg.len, msg.seq);
#endif

            if( msg.sysid >= 50 ) continue;

            // MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 33
            if( msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT ) {
                mavlink_global_position_int_t gp;
                mavlink_msg_global_position_int_decode(&msg, &gp);

                /*
                printf("    time_boot_ms        = %d\n", gp.time_boot_ms);
                printf("    lat                 = %d\n", gp.lat);
                printf("    lon                 = %d\n", gp.lon);
                printf("    alt                 = %d\n", gp.alt);
                printf("    relative_alt        = %d\n", gp.relative_alt);
                printf("    vx                  = %d\n", gp.vx);
                printf("    vy                  = %d\n", gp.vy);
                printf("    vz                  = %d\n", gp.vz);
                printf("    hdg                 = %d\n\n", gp.hdg);
                */

                if( 0 == i ) {
                    lat0 = 1.0 * gp.lat / 1e7;
                    lng0 = 1.0 * gp.lon / 1e7;
                } else {
                    lat1 = 1.0 * gp.lat / 1e7;
                    lng1 = 1.0 * gp.lon / 1e7;

                    calc_earth_offset( lng0, lat0, lng1, lat1, dx0, dy0);
                    calcLngLatDistance(lng0, lat0, lng1, lat1, dx1, dy1, 0);
                    calcLngLatDistance(lng0, lat0, lng1, lat1, dx2, dy2, 1);

                    printf("dx, dy = (%13.6f, %13.6f, %13.6f), (%13.6f, %13.6f, %13.6f)\n",
                           dx0, dx1, dx2,
                           dy0, dy1, dy2);
                }

                i++;
            }

        }
    }

    delete uart;

    return 0;
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int conv_FlightData(CParamArray *pa)
{
    FILE                *fp;
    string              fn_in, fn_out;
    int                 dataType;
    int                 uavID;

    mavlink_message_t   msg;
    mavlink_status_t    status;

    uint8_t             ub;

    UAS_Base            uas;

    // get input arguments
    fn_in       = svar.GetString("fn_in", "");          // input MVD file name
    fn_out      = svar.GetString("fn_out", "");         // output FLD file name
    dataType    = svar.GetInt("datatype", 0);           // output data type
        //      0 - FLD (normal format)
        //      1 - simplified FLD (for SDK)
        //      2 - GSLAM format
    uavID       = svar.GetInt("uavID", 1);              // UAV id

    if( fn_in.size() <= 0 || fn_out.size() <= 0 ) {
        dbg_pe("Please input input/output file!");
        return -1;
    }

    // parse input MVD file
    fp = fopen(fn_in.c_str(), "rb");
    if( fp == NULL ) {
        dbg_pe("Input file can not open! %s", fn_in.c_str());
        return -2;
    }


    // begin receiving
    while( !feof(fp) ) {
        ub = fgetc(fp);

        // parse received data
        if( mavlink_parse_char(MAVLINK_COMM_0, ub, &msg, &status) ) {
            if( msg.sysid == uavID ) {
                uas.parseMavlinkMsg(&msg);
            }
        }
    }

    fclose(fp);

    // convert to POS_Data
    if( dataType == 0 ) {
        uas.m_posData.save(fn_out.c_str());
    }

    // convert to FlightData_Simp
    if( dataType == 1 ) {
        int32_t nData = uas.m_posData.length();
        vector<FlightData_Simp> fdArray;

        printf("get %d POS_Data\n", nData);
        fdArray.resize(nData);

        for(int i=0; i<nData; i++) {
            POS_Data &pd = uas.m_posData.at(i);
            FlightData_Simp fd;

            fd.lat      = pd.lat;
            fd.lng      = pd.lng;
            fd.alt      = pd.altitude;
            fd.H        = pd.h;
            fd.yaw      = pd.ahrs.yaw;
            fd.pitch    = pd.ahrs.pitch;
            fd.roll     = pd.ahrs.roll;

            fdArray[i] = fd;
        }

        // save to file
        fp = fopen(fn_out.c_str(), "wb");
        if( fp == NULL ) {
            dbg_pe("Cannot open output file: %s", fn_out.c_str());
            return -1;
        }

        fwrite(&nData, sizeof(int32_t), 1, fp);
        fwrite(fdArray.data(), sizeof(FlightData_Simp)*nData, 1, fp);

        fclose(fp);
    }

    if( dataType == 2)  {
        ofstream ofs(fn_out.c_str());
        if(!ofs.is_open())
        {
            dbg_pe("Cannot open output file: %s", fn_out.c_str());
            return -1;
        }
        int32_t nData = uas.m_posData.length();
        vector<FlightData_Simp> fdArray;

        printf("get %d POS_Data\n", nData);

        for(int i=0; i<nData; i++) {
            POS_Data &pd = uas.m_posData.at(i);

            ofs<<pi::dtos(pd.time.toTimeStampF())<<" "<<pi::dtos(pd.lng,8)
                <<" "<<pi::dtos(pd.lat,8)<<" "<<pi::dtos(pd.altitude)<<" "
                <<pd.nSat<<" "<<pd.ahrs.yaw<<" "<<pd.ahrs.pitch<<" "<<pd.ahrs.roll<<"\n";
        }
    }
    return 0;
}

int test_FlightData_Recv(CParamArray *pa)
{
    RSocket             socket;

    string              addr = "127.0.0.1";
    int                 port = 10023;

    FlightData_Simp     fd;
    int                 fdSize;

    int                 ret = 0;

    addr = svar.GetString("addr", addr);
    port = svar.GetInt("port", port);

    if( 0 != socket.startClient(addr, port) ) {
        dbg_pe("Failed to connect %s", addr.c_str());
        return -1;
    }

    fdSize = sizeof(FlightData_Simp);

    while( 1 ) {
        ret = socket.recv((uint8_t*)&fd, fdSize);
        if( ret < 0 ) {
            dbg_pe("Connection lost!");
            break;
        } else if( ret != fdSize ) {
            dbg_pw("Received data not correct!");
            break;
        }

        ret = fd.checkCRC();
        printf("Receive FlightData, coorect = %d\n", ret);
        fd.print();
        printf("\n");
    }


    return 0;
}


int test_MissionData_Trans_server(CParamArray *pa)
{
    MissionData_Transfer    mdt;
    MissionData_Simp        md;

    string              addr = "225.0.0.11";
    int                 port = 10024;

    int                 ret = 0;

    addr = svar.GetString("addr", addr);
    port = svar.GetInt("port", port);

    ret = mdt.begin(addr, port, 1);
    if( 0 != ret ) {
        dbg_pe("Failed to connect %s", addr.c_str());
        return -1;
    }

    md.version = 1;
    md.uavID = 1;
    md.lat = 10;
    md.lng = 20;
    md.alt = 30;
    md.missionIdx = 0;
    md.missionType = MissionData_Simp::MT_WAYPOINT;

    while( 1 )
    {
        md.missionIdx ++;
        md.calcCRC();

        mdt.sendMission(&md);

        printf("SEND: [%6d]\n", md.missionIdx);
        md.print();
        printf("\n");

        tm_sleep(500);
    }

    return 0;
}

int test_MissionData_Trans_client(CParamArray *pa)
{
    MissionData_Transfer    mdt;
    MissionData_Simp        md;

    string              addr = "225.0.0.11";
    int                 port = 10024;

    int                 ret = 0;

    addr = svar.GetString("addr", addr);
    port = svar.GetInt("port", port);

    ret = mdt.begin(addr, port, 0);
    if( 0 != ret ) {
        dbg_pe("Failed to connect %s", addr.c_str());
        return -1;
    }

    while( 1 )
    {
        if( mdt.recvMission(&md) == 0 )
        {
            md.print();
        }
        else
        {
            tm_sleep(10);
        }
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_VirtualUAV(CParamArray *pa)
{
    HAL_JSComobo        js;

    VirtualUAV_Manager  uavManager;
    VirtualUAV_Quad     *uavQuad;

    FlightGear_Transfer fgTrans;
    FGNetFDM            fdm;

    // create virtual UAV
    uavQuad = new VirtualUAV_Quad();
    uavQuad->ID         = 0;
    //uavQuad->gpLat      = 34.5606826577973;
    //uavQuad->gpLon      = 112.295299172401;
    uavQuad->gpLat      = 37.61522300000000;
    uavQuad->gpLon      = -122.38997899999998;
    uavQuad->gpAlt      = 140;
    uavQuad->gpH        = 20;
    uavQuad->homeLat    = 34.5606826577973;
    uavQuad->homeLng    = 112.295299172401;
    uavQuad->homeAlt    = 120;

    uavManager.addUAV(uavQuad);

    js.open(svar.GetString("FastGCS.joystick", "Joystick.Thrustmaster_THX"));
    uavManager.setJoystick(&js);

    fgTrans.connect();

    // endless loop
    Rate r(10);
    while(1) {
        uavQuad->toFlightGear(&fdm);
        fgTrans.trans(&fdm);

        r.sleep();
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_mobileGPS(CParamArray *pa)
{
    GPS_Reader      gpsReader;
    UART            uart;
    POS_Data        gpsData;

    // open UART port
    uart.port_name = svar.GetString("port", "/dev/ttyUSB0");;
    uart.baud_rate = svar.GetInt("baud", 115200);
    if( 0 != uart.open() ) {
        dbg_pe("Can not open UART port: %s\n", uart.port_name.c_str());
        return -1;
    }

    // start GPS reader
    gpsReader.m_uart = &uart;
    gpsReader.begin();

    // loop forever
    while( 1 ) {
        if( gpsReader.getData(gpsData) == 0 ) {
            gpsData.print();
            printf("\n");
        } else {
            tm_sleep(5);
        }
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int test_plugin(CParamArray *pa)
{
    // begin Qt
    int     argc = svar.i["argc"];
    char**  argv = (char**) SvarWithType<void*>::instance()["argv"];

    QApplication app(argc, argv);

    // begin load plugin
    QPluginLoader loader;
    QString pluginName = "/home/bushuhui-data/msdk/uav/uav_sys/PIS/PIS/libs/libpi_TestPlugin.so";

    loader.setFileName(pluginName);
    bool res = loader.load();
    QObject *plugin = loader.instance();

    printf("plugin: %s\n", pluginName.toStdString().c_str());
    printf("    loader res = %d, plugin pointer = %llx\n", res, plugin);

    if ( plugin ) {
        printf("    load plugin successfully!\n");
    } else {
        printf("    load plugin failed!\n");
    }

    return 0;
}
